#include "gridfollower.h"


GridFollower::GridFollower()
{
  noIntersect=0;
  SensorStatus=0;
  Lsensor1=Lsensor2=Lsensor3=0;
  Rsensor1=Rsensor2=Rsensor3=0;
  PLsensor1=PLsensor2=PLsensor3=0;
  PRsensor1=PRsensor2=PRsensor3;
  Csensor=PCsensor=0;
  ReadingCounts=0; 
}

void GridFollower::attachGridSensor(int flsensor,int frsensor,int blsensor,int brsensor)
{
  GridFollower::flsensor=flsensor;
  GridFollower::frsensor=frsensor;
  GridFollower::blsensor=blsensor;
  GridFollower::brsensor=brsensor;
}

void GridFollower::readGridSensor()
{
  GridFollower::flsensorval=digitalRead(flsensor);
  GridFollower::frsensorval=digitalRead(frsensor);
  GridFollower::blsensorval=digitalRead(blsensor);
  GridFollower::brsensorval=digitalRead(brsensor);
}


bool GridFollower::isIntersection()
{
  //      readSensor();
  ReadingCounts++;
  if(sensorVal(0))// && !PCsensor)    //Detecting Rising Edge
  { 
    Csensor=1;
  }
  if(sensorVal(1))// && !PRsensor1)
  {
    Rsensor1=1;
  }
  if(sensorVal(2))// && !PRsensor2)
  {
    Rsensor2=1;
  }
  if(sensorVal(3))// && !PRsensor3)
  {
    Rsensor3=1;

  }
  if(sensorVal(-1))// && !PLsensor1)
  {
    Lsensor1=1;
  }
  if(sensorVal(-2))// && !PLsensor2)
  {
    Lsensor2=1;
  }
  if(sensorVal(-3))// && !PLsensor3)
  {
    Lsensor3=1;

  }
  PCsensor=sensorVal(0);
  PLsensor1=sensorVal(-1);
  PLsensor2=sensorVal(-2);
  PLsensor3=sensorVal(-3);
  PRsensor1=sensorVal(1);
  PRsensor2=sensorVal(2);
  PRsensor3=sensorVal(3);

  SensorStatus = Csensor + Lsensor1 + Lsensor2 + Lsensor3 + Rsensor1 + Rsensor2 + Rsensor3;

  if(ReadingCounts==1)
  {
    ReadingCounts=0;
    Csensor=0;
    Lsensor1=0;
    Lsensor2=0;
    Lsensor3=0;
    Rsensor1=0;
    Rsensor2=0;
    Rsensor3=0;
  }
  //SensorStatus = sensorVal(0) + sensorVal(1) + sensorVal(2) + sensorVal(3) + sensorVal(-1) + sensorVal(-2) + sensorVal(-3);
//  Serial.print("SensorStatus= ");  Serial.print(SensorStatus);
  if(SensorStatus>=3 && flag==0)
  {
    flag=1;
    return 1;
  }
  else 
  {
    if(SensorStatus<3)
      flag=0;
    return 0;
  }

}

void GridFollower::gotoNode(int node)
{
  noIntersect=0;
  if(node<0)
  {
    node=-node;
    rotateDegrees(180);
  }
  
  moveForward(100,100,0,0);
  while((noIntersect!=node))
  {

    followLine();
    if(isIntersection())
    {
      noIntersect++;
    }
//    Serial.print("   Intersections= ");    Serial.print(noIntersect);
  }
  stop();
//  moveBackward(200,200,0,0);
  noIntersect=0;

}

void GridFollower::rotateDegrees(int angle)
{
  if(angle==90)
  {
//    Serial.println("Starting Turn");
//    Serial.print(sensorVal(-2));
    
    Base::rotateRight(MEAN_SPEED_TURNING,MEAN_SPEED_TURNING,0,0);
    while(!sensorVal(-3) && !sensorVal(-2))  //Turning the Bot Right until the left sensors Detects the current Line
    {
      readSensor();
    }
//    Serial.println("Current Line Detected");
    while(sensorVal(-3) && sensorVal(-2))      //Turning Right until the left sensors Leaves the current Line
    {
      readSensor();
    }
//    Serial.println("Current Line Left");
    while((!sensorVal(3)) && (!sensorVal(2)))  //Turning Right until the Right sensors detects the next Line
    {
      readSensor();
    }
//    Serial.println("Next Line Detected");
    
    while(sensorVal(3) && sensorVal(2))
    {
      readSensor();
    }
//    Serial.println("Next line Left by Right Sensors");
    
    while((!sensorVal(-1)) && (!sensorVal(0)) && (!sensorVal(1)))
    {
      readSensor();
    }
//    Serial.println("On Line");
    stop();
//    Serial.println("Turning Completed");
  }

  else if(angle== -90)
  {
    Base::rotateLeft(MEAN_SPEED_TURNING,MEAN_SPEED_TURNING,0,0);
    while(!sensorVal(3) && !sensorVal(2))  //Turning the Bot Left until the Right sensors Detects the current Line
    {
      readSensor();
    }
    while(sensorVal(3) && sensorVal(2))      //Turning Left until the Right sensors Leaves the current Line
    {
      readSensor();
    }
    while((!sensorVal(-3)) && (!sensorVal(-2)))  //Turning Left until the Left sensors detects the next Line
    {
      readSensor();
    }
    while(sensorVal(-3) && sensorVal(-2))
    {
      readSensor();
    }
    while((!sensorVal(-1)) && (!sensorVal(0)) && (!sensorVal(1)))
    {
      readSensor();
    }
    stop();
  }
  else if(angle==180)
  {
    rotateDegrees(90);
    rotateDegrees(90);
  }
  else if(angle==-180)
  {
    rotateDegrees(-90);
    rotateDegrees(-90);
  }
}

void GridFollower::followGrid(int a,int b)
{
  GridFollower::x=a;
  GridFollower::y=b;
  gotoNode(y);
  delay(DELAY_AT_NODES);
  if(x>0)
  {
    rotateDegrees(90);
    gotoNode(x);
  delay(DELAY_AT_NODES);
  }
  if(x<0)
  {
    rotateDegrees(-90);
    gotoNode(-x);
  delay(DELAY_AT_NODES);
  }
}




